#pragma once
#include <vector>
#include <memory>
#include "nml_intf/zucpos2.h"

class simple_tp_t;

namespace axisgroup {
class StopPlanner
{
public:
    StopPlanner() = default;
    ~StopPlanner() = default;
    int init(RobJointVal cur_pos, RobJointVal cur_vel, int joint_num, double ts);
    int update(RobJointVal& stop_cmd);
    bool is_stopped();

private:
    std::vector<std::shared_ptr<simple_tp_t>> stop_tp_;
    int joint_num_ = 6;
    double ts_ = 0.001;
};
}  // namespace axisgroup
